/**
 * @file demo.cpp
 * @brief Huaray驱动测试程序
 * @author BG2EDG (928330305@qq.com)
 * @date 2022-04-16
 *
 * @copyright Copyright (C) 2022, HITCRT_VISION, all rights reserved.
 *
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Author  <th>Description
 * <tr><td>2022-04-16 <td>BG2EDG  <td>
 * <tr><td>2022-05-15 <td>BG2EDG  <td>Timepoint使用时加命名空间
 * <tr><td>2022-05-15 <td>BG2EDG  <td>onGet函数，进入回调后立即执行
 * </table>
 */
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <yaml-cpp/yaml.h>

#include <atomic>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <functional>
#include <iostream>
#include <list>
#include <mutex>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <openvino/openvino.hpp>
#include <thread>

#include "HuarayCam.h"
#include "TCPClient.h"  //添加到头文件中
#include "aim_assist/ArmorBase.h"
#include "aim_assist/ArmorCalculator.h"
#include "aim_assist/ArmorClassifierBase.h"
#include "aim_assist/ArmorDetector.h"
#include "aim_assist/ArmorTrackerBase.h"
#include "aim_assist/BarDetector.h"
#include "aim_assist/TrackBase.h"
#include "basic/Basic.h"
#include "serial/infantry/serialFactory.h"
#include "serial/infantry/serialInfantry.h"

typedef class MyClass MyClass;
std::mutex FRAME_MUTEX;  // 数据锁
cv::Mat IMAGE;           // 作为共享数据 在不同线程中

std::atomic<int> JUDGE_EXIT(1);    // 退出和暂停标志
std::atomic<int> JUDGE_VISION(1);  // 可视化标志

int INPUT_VALUE = 1;    // 输入指令double
double START_TIME = 0;  // 计算时间的时间节点
double END_TIME = 0;    // 计算时间的时间节点

hitcrt::Color COLOR;  // 颜色初始化
hitcrt::ROI ROI;      // ROI初始化
hitcrt::TimePoint START_TIME_1;
float PITCH_DEGREE, YAW_DEGREE;
// yaml读取参数
YAML::Node config = YAML::LoadFile("../param/config.yaml");
// 识别类初始化参数
hitcrt::BarDetector BAR_DETECTOR(1,       // 轮廓数量最小
                                 2500,    // 轮廓数量最大
                                 2,       // 灯条面积最小
                                 100000,  // 灯条面积最大
                                 2,       // 灯条长度最小
                                 1000,    // 灯条长度最大
                                 0.01,    // 宽长比最小
                                 0.5,     // 宽长比最大
                                 1.1,     // 颜色比例
                                 1.5,     // 水平倾斜
                                 55       // 颜色差
);
hitcrt::ArmorAssembler ASSEMBLER(0.30,   // 平行偏差
                                 1.428,  // 长度比
                                 0.7,    // 水平偏差
                                 1.5,    // 宽长比最小
                                 5.5,    // 宽长比最大
                                 1.18,   // 边角最小
                                 20
                                 // 边角最大

);

hitcrt::ArmorDetector ARMOR_DETECTOR(165);

std::vector<std::shared_ptr<hitcrt::Track>> TRACKS;
hitcrt::ArmorTrackerBase ARMOR_TRACKER(10, 5, TRACKS);
// vofas+用
std::shared_ptr<hitcrt::TCPClient> m_web;
// 串口初始化
std::unique_ptr<hitcrt::SerialCom> serial = std::unique_ptr<hitcrt::SerialCom>(
    hitcrt::SerialFactory::createSerial(3, "auto", 460800));
void dataVision(const hitcrt::ArmorTrackerBase& ARMOR_TRACKER,
                cv::Mat& imageTemp);
void fpsVision(const double startTime, const double endTime,
               cv::Mat& imageTemp);
void noUse(const hitcrt::camera::TimePoint& timeStamp,
           const cv::Mat& frameImage) {};
// 自定义的类
class RobotDemo {
   public:
    // 用于帧回调的成员函数签名必须满足void(const int64_t timeStamp, const
    // cv::Mat& frameImage)
    // timsStamp记录的是从1970时间起的纳秒数，获取方法如下：
    //    auto now = std::chrono::system_clock::now();
    //    auto timeStamp = now.time_since_epoch().count();
    // frameImage是存储RGB8的cv::MatTrjState::INIT

    // 回调函数
    void apply(const hitcrt::camera::TimePoint& timeStamp,
               const cv::Mat& frameImage) {
        // 初始化装甲板
        std::vector<hitcrt::Armor> armors;
        cv::Mat imageTemp;  // 暂存图像
        double pitchDegree, yawDegree;
        // 根据时间差 计算帧率
        START_TIME = END_TIME;
        END_TIME = cv::getTickCount();
        if (!frameImage.empty()) {
            imageTemp = frameImage.clone();
            hitcrt::Frame frame(imageTemp,
                                timeStamp);  // 这里测试 timeStamp 可不可用
            startTime = hitcrt::Clock::now();
            // 如果可视化 则进入下面
            if (1) {  // JUDGE_VISION % 2
                if (!frame.empty()) {
                    // 帧率可视化
                    fpsVision(START_TIME, END_TIME, imageTemp);
                    // 装甲板识别
                    ARMOR_DETECTOR.detect(frame, COLOR, ROI, BAR_DETECTOR,
                                          ASSEMBLER, armors);
                    // 传入输入的Pitch和Yaw角度值
                    FRAME_MUTEX.lock();
                    pitchDegree = -PITCH_DEGREE;
                    yawDegree = -YAW_DEGREE;
                    FRAME_MUTEX.unlock();
                    hitcrt::ArmorCalculator ARMOR_CALCULATOR(
                        pitchDegree, yawDegree,
                        config["hero_height"].as<double>(),
                        config["hero_width"].as<double>(),
                        config["infantry_width"].as<double>());
                    // 位姿转换
                    ARMOR_CALCULATOR.rotationCalculation(armors, imageTemp);

                    // 轨迹追踪
                    ARMOR_TRACKER.apply(startTime, armors);

                    if (JUDGE_VISION % 2) {
                        dataVision(ARMOR_TRACKER, imageTemp);
                    }
                }
                // vofa+用
                {
                    for (int i = 0; i < ARMOR_TRACKER.m_tracks.size(); i++) {
                        if (ARMOR_TRACKER.m_tracks[i]->m_trjState ==
                                hitcrt::NORMAL &&
                            ARMOR_TRACKER.m_tracks[i]->m_armors.size()) {
                            std::ostringstream sendWebData;
                            sendWebData << "Data:"
                                        // 一直发送的数据
                                        << (float)ARMOR_TRACKER.m_tracks[i]->m_armors.back().m_filterPointmeasure.x<< ","
                                        << (float)ARMOR_TRACKER.m_tracks[i]->m_armors.back().m_filterPointmeasure.y << ","
                                        << (float)ARMOR_TRACKER.m_tracks[i]->m_armors.back().m_filterPointmeasure.z << ","
                                        << (float)ARMOR_TRACKER.m_tracks[i]->m_armors.back().m_measurePoint.x<< ","
                                        << (float)ARMOR_TRACKER.m_tracks[i]->m_armors.back().m_measurePoint.y << ","
                                        << (float)ARMOR_TRACKER.m_tracks[i]->m_armors.back().m_measurePoint.z << ","
                                        << "\n";
                            m_web->sendData(sendWebData);
                        }}
                }
                        /*串口数据收发*/
                        {
                            bool g_send_findFlag = false;
                            bool g_send_shootFlag = false;
                            bool g_send_cameraFlag = false;
                            bool g_send_fiveFlag = false;
                            float g_send_pitchDegree = 0, g_send_yawDegree = 0;
                            for (int i = 0; i < ARMOR_TRACKER.m_tracks.size();
                                 i++) {
                                if (ARMOR_TRACKER.m_tracks[i]->m_trjState ==
                                    hitcrt::NORMAL) {
                                    g_send_pitchDegree =
                                        ARMOR_TRACKER.m_tracks[i]
                                            ->m_armors.back()
                                            .m_yawSend /
                                        CV_PI * 180;
                                    g_send_yawDegree =
                                        ARMOR_TRACKER.m_tracks[i]
                                            ->m_armors.back()
                                            .m_pitchSend /
                                        CV_PI * 180;
                                }
                            }
                            std::cout << "g_send_pitchDegree"
                                      << g_send_pitchDegree << std::endl;
                            std::cout << "g_send_yawDegree" << g_send_yawDegree
                                      << std::endl;

                            int g_send_forceNum = 0;
                            bool g_send_enemySpin = false;
                            bool g_send_attackSpin = false;
                            float g_debug_0 = 0;
                            // 发送部分
                            std::vector<boost::any> send_data;
                            std::vector<float> num;
                            hitcrt::Float2uchar stateFlag{};
                            stateFlag.ch[0] = (unsigned char)
                                g_send_findFlag;  // 是否找到装甲板，bool
                            stateFlag.ch[1] = (unsigned char)
                                g_send_shootFlag;  // bool，这周任务用不上，设为false
                            stateFlag.ch[2] = (unsigned char)
                                g_send_cameraFlag;  // bool，这周任务用不上，设为false
                            stateFlag.ch[3] = (unsigned char)
                                g_send_fiveFlag;  // bool，这周任务用不上，设为false
                            num.emplace_back(stateFlag.fl);
                            num.emplace_back(float(
                                g_send_pitchDegree));  // float，发送角度，角度制
                            num.emplace_back(float(
                                g_send_yawDegree));  // float，发送角度，角度制
                            hitcrt::Float2uchar AAstate{};
                            AAstate.ch[0] = (unsigned char)
                                g_send_forceNum;  // int，这周任务用不上，设为false
                            AAstate.ch[1] = (unsigned char)
                                g_send_enemySpin;  // bool，这周任务用不上，设为false
                            AAstate.ch[2] = (unsigned char)
                                g_send_attackSpin;  // bool，这周任务用不上，设为false
                            num.emplace_back(AAstate.fl);
                            num.emplace_back(
                                g_debug_0);  // 和电控联调时用的变量，暂时没用到，随便传float
                            send_data.emplace_back((
                                unsigned char)26);  // 暂时没用上，意义为传输长度
                            send_data.emplace_back(num);
                            serial->send(send_data);
                            // std::cout<<"send_data.size()"<<num.size()<<std::endl;
                        }
                        // 图像赋值给全局变量 进行图显
                        FRAME_MUTEX.lock();
                        IMAGE = imageTemp.clone();
                        FRAME_MUTEX.unlock();
                    }
                    else {
                        std::cout << "ERROR" << std::endl;
                    }
                    // 暂停
                    while (JUDGE_EXIT % 2 == 0) {
                    }
                }
            };

            void onGet() {
                RobotDemo::onGetTime = std::chrono::steady_clock::now();
                // std::cout << "onGet!" << std::endl;
            }
            static hitcrt::camera::TimePoint startTime;
            static hitcrt::camera::TimePoint onGetTime;
        };
        hitcrt::camera::TimePoint RobotDemo::startTime;
        hitcrt::camera::TimePoint RobotDemo::onGetTime;

        class ConnectFunctor {
           public:
            // 用于断线重连回调的成员函数签名必须满足void()
            // 相机断线主动调用该函数，建议在里面新开一个线程或以某种方式将断线信息发送给其他线程
            // 去监控相机的open或grab变量，时间过长自动退出程序
            void camOffLine() { std::cout << OFF_STR << " Line!" << std::endl; }
            // 相机重连会先调用该函数，然后调用驱动里的retry()函数重启相机，建议在里面与断线配合
            // 要保证retry成功，即确实isOpen或者isGrabbing，不然应该退出程序
            void camOnLine() { std::cout << ON_STR << " Line!" << std::endl; }

           private:
            // bind使用bind绑定成员函数和对象后，成员函数可以访问到指定对象的成员变量
            const std::string OFF_STR = "Off";
            const std::string ON_STR = "On";
        };

        // 抓取并处理程序的线程函数
        void frameCapture() {
            // 实例化相机
            RobotDemo robot;
            ConnectFunctor func;
            // 相机参数设置
            // 注意此处boost::bind的用法，将成员函数与对象绑定，待输入参数用占位符_1替代
            hitcrt::camera::HuarayParams camParams(
                boost::bind(&RobotDemo::apply, &robot, _1, _2), 1,
                hitcrt::camera::Mode::STREAM,
                config["CamParam"]["Width"].as<double>(),
                config["CamParam"]["Height"].as<double>(),
                config["CamParam"]["BlackLevel"].as<double>(),
                config["CamParam"]["Brightness"].as<double>(),
                config["CamParam"]["DigitalShift"].as<double>(),
                config["CamParam"]["Sharpness"].as<double>(),
                config["CamParam"]["ExposureTime"].as<double>(),
                config["CamParam"]["Gamma"].as<double>(),
                config["CamParam"]["GainRaw"].as<double>(),
                {config["CamParam"]["BalanceRatio"][0].as<double>(),
                 config["CamParam"]["BalanceRatio"][1].as<double>(),
                 config["CamParam"]["BalanceRatio"][2].as<double>()});
            // 参数设置还可通过set方法实现
            camParams.setOffLineFunc(
                boost::bind(&ConnectFunctor::camOffLine, &func));
            camParams.setOnLineFunc(
                boost::bind(&ConnectFunctor::camOnLine, &func));
            camParams.setOnGet(boost::bind(&RobotDemo::onGet, &robot));

            // 直接使用HuarayParams对象初始化Huaray对象，则立即执行相机初始化
            hitcrt::camera::Huaray cam(camParams);
            if (cam.isGrabbing()) {
                std::cout << "Camera is grabbing!" << std::endl;
            } else {
                std::cout << "Camera is not grabbing!" << std::endl;
            }
            // 也可以先实例化对象，然后使用initiate执行初始化
            // hitcrt::camera::Huaray cam;
            // if(cam.initiate(camParams)){
            //     std::cout << "Camera initiate success!" << std::endl;
            // }else{
            //     std::cout << "Camera initiate failed!" << std::endl;
            //     return -1;
            // }

            // 向相机查询当前参数，返回包含相机信息的std::unique_ptr
            // 此处使用auto自动进行型别推导
            auto param = cam.get();
            // 向终端格式化输出参数
            (*param).show();
            while (JUDGE_EXIT != -1) {
            }
            // 阻塞主线程，等到键盘按下
            std::cout << "Press a key to reset params ..." << std::endl;
            // getchar();

            // 快速参数重置
            // 前五个参数都不会修改
            // hitcrt::camera::HuarayParams camParams3(
            //     boost::bind(&noUse, _1, _2), 1, hitcrt::camera::Mode::STREAM,
            //     1280, 1024, 20, 60, 0, 70, 1000.0, 0.7, 1.0,
            //     {1.0, 1.0, 1.0});
            // cam.resetLite(camParams3);

            // // 向相机查询当前参数，返回包含相机信息的std::unique_ptr
            // // 此处使用auto自动进行型别推导
            // param = cam.get();
            // // 向终端格式化输出参数
            // (*param).show();
            // while (JUDGE_EXIT != -1) {
            // }
            // // 阻塞主线程，等到键盘按下
            // std::cout << "Press a key to reset params ..." << std::endl;
            // //getchar();

            // 判断相机是否开启，如有其他程序占用也会返回false
            std::cout << "IsOpen: " << cam.isOpen() << std::endl;
            // 判断相机是否正常抓图
            std::cout << "IsGrabbing: " << cam.isGrabbing() << std::endl;

            std::cout << "Press a key to exit..." << std::endl;

            std::cout << JUDGE_EXIT << std::endl;
            // 停止抓图，销毁相机句柄
            // 为防止用户忘记，在cam的析构函数里也设置了这一操作
            // 但最好加上
            cam.terminate();
        }

        /**
         * @brief 图显程序
         * @author fx_03 (18249293810@qq.com)
         */
        void framePlay() {
            while (JUDGE_EXIT != -1) {
                FRAME_MUTEX.lock();
                cv::Mat imagePlay = IMAGE.clone();
                FRAME_MUTEX.unlock();
                if (!(imagePlay.empty())) {
                    cv::imshow("image", imagePlay);
                    cv::waitKey(100);
                } else {
                    std::cout << "ERROR!!!" << std::endl;
                }
            }
            std::cout << "framePlay EXIT" << std::endl;
        }

        /**
         * @brief 交互程序
         * @author fx_03 (18249293810@qq.com)
         */
        void frameInter() {
            while (JUDGE_EXIT != -1) {
                if (cv::waitKey(30)) {
                    INPUT_VALUE = getchar();
                    if (INPUT_VALUE == 'p') {
                        JUDGE_EXIT.store(JUDGE_EXIT.load() + 1);  // 暂停
                    } else if (INPUT_VALUE == 'q') {
                        JUDGE_EXIT.store(-1);  // 退出
                        break;
                    } else if (INPUT_VALUE == 'v') {
                        JUDGE_VISION.store(JUDGE_VISION.load() +
                                           1);  // 可视化 奇偶判断
                    }
                }
            }
        }

        // 接收数据
        void SerialReceive() {
            // 接收和读取数据，另开线程
            while (1) {
                std::vector<boost::any> receive_data;
                serial->receive(receive_data);
                std::vector<float> data;
                data = boost::any_cast<std::vector<float>>(receive_data.at(1));
                /// 前二位为角度信息
                double tempPitchDegree, tempYawDegree;
                tempPitchDegree = data[0];
                tempYawDegree = -data[1];

                FRAME_MUTEX.lock();
                PITCH_DEGREE = -tempPitchDegree;
                YAW_DEGREE = -tempYawDegree;
                FRAME_MUTEX.unlock();

                // std::cout << "temp_PitchDegree: " << tempPitchDegree <<
                // std::endl; std::cout << "temp_YawDegree: " << tempYawDegree
                // << std::endl;
            }
        }

        // 发送数据

        int main() {
            std::string IP_ADDRESS;
            int IP_PORT;

            IP_ADDRESS = "127.0.0.1";  // 本地用127.0.0.1、小电脑用192.168.50.1
            IP_PORT = 1347;

            boost::thread m_webRunThread, m_webSendDataThread;

            m_web = std::make_unique<hitcrt::TCPClient>(IP_ADDRESS, IP_PORT);
            m_webRunThread = boost::thread(
                boost::bind(&hitcrt::TCPClient::runServer, &(*m_web)));
            m_webSendDataThread = boost::thread(
                boost::bind(&hitcrt::TCPClient::sendDataThread, &(*m_web)));

            boost::thread frameInterThread = boost::thread(frameInter);
            boost::thread framePlayThread = boost::thread(framePlay);
            boost::thread frameCaptureThread = boost::thread(frameCapture);
            boost::thread serialReceive = boost::thread(SerialReceive);

            frameInterThread.join();
            framePlayThread.join();
            frameCaptureThread.join();
            serialReceive.join();
            return 0;
        }
        /**
         * @brief 帧率可视化
         * @param[in] startTime      MyParam doc
         * @param[in] endTime        MyParam doc
         * @param[in] imageTemp      MyParam doc
         * @author fx_03 (18249293810@qq.com)
         */
        void fpsVision(double startTime, double endTime, cv::Mat& imageTemp) {
            double fps;
            std::string fpsText;
            fps = cv::getTickFrequency() / (endTime - startTime);
            fpsText = "FPS: " + std::to_string((int)fps);
            cv::putText(imageTemp, fpsText, cv::Point(10, 55),
                        cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(0, 255, 0),
                        1);
        }
        void dataVision(const hitcrt::ArmorTrackerBase& ARMOR_TRACKER,
                        cv::Mat& imageTemp) {
            cv::Scalar BLUE(255, 0, 0), GREEN(0, 255, 0), RED(0, 0, 255),
                colorTemp;

            for (int i = 0; i < ARMOR_TRACKER.m_tracks.size(); i++) {
                if (ARMOR_TRACKER.m_tracks[i]->m_trjState == hitcrt::NORMAL &&
                    ARMOR_TRACKER.m_tracks[i]->m_armors.size()) {
                    colorTemp = GREEN;
                    hitcrt::Armor armor =
                        ARMOR_TRACKER.m_tracks[i]->m_armors.back();
                    cv::line(imageTemp, armor.m_topLeft, armor.m_topRight,
                             colorTemp, 1, 8, 0);
                    cv::line(imageTemp, armor.m_bottomLeft, armor.m_bottomRight,
                             colorTemp, 1, 8, 0);
                    cv::line(imageTemp, armor.m_topLeft, armor.m_bottomLeft,
                             colorTemp, 1, 8, 0);
                    cv::line(imageTemp, armor.m_topRight, armor.m_bottomRight,
                             colorTemp, 1, 8, 0);
                    cv::putText(imageTemp, std::to_string(armor.m_pattern),
                                armor.m_centerUV, cv::FONT_HERSHEY_SIMPLEX, 1,
                                colorTemp, 1);

                    cv::putText(
                        imageTemp,
                        std::to_string((int)armor.m_filterPointmeasure.x),
                        cv::Point2f(250, 55 + 50 * i), cv::FONT_HERSHEY_SIMPLEX,
                        0.8, GREEN, 1);
                    cv::putText(
                        imageTemp,
                        std::to_string((int)armor.m_filterPointmeasure.y),
                        cv::Point2f(250, 80 + 50 * i), cv::FONT_HERSHEY_SIMPLEX,
                        0.8, GREEN, 1);
                    cv::putText(
                        imageTemp,
                        std::to_string((int)armor.m_filterPointmeasure.z),
                        cv::Point2f(250, 105 + 50 * i),
                        cv::FONT_HERSHEY_SIMPLEX, 0.8, GREEN, 1);
                } else if (ARMOR_TRACKER.m_tracks[i]->m_trjState ==
                               hitcrt::INIT &&
                           ARMOR_TRACKER.m_tracks[i]->m_armors.size()) {
                    colorTemp = BLUE;
                    hitcrt::Armor armor =
                        ARMOR_TRACKER.m_tracks[i]->m_armors.back();
                    cv::line(imageTemp, armor.m_topLeft, armor.m_topRight,
                             colorTemp, 1, 8, 0);
                    cv::line(imageTemp, armor.m_bottomLeft, armor.m_bottomRight,
                             colorTemp, 1, 8, 0);
                    cv::line(imageTemp, armor.m_topLeft, armor.m_bottomLeft,
                             colorTemp, 1, 8, 0);
                    cv::line(imageTemp, armor.m_topRight, armor.m_bottomRight,
                             colorTemp, 1, 8, 0);
                    cv::putText(imageTemp, std::to_string(armor.m_pattern),
                                armor.m_centerUV, cv::FONT_HERSHEY_SIMPLEX, 1,
                                colorTemp, 1);

                } else if (ARMOR_TRACKER.m_tracks[i]->m_trjState ==
                               hitcrt::FADE &&
                           ARMOR_TRACKER.m_tracks[i]->m_armors.size()) {
                    colorTemp = RED;
                    hitcrt::Armor armor =
                        ARMOR_TRACKER.m_tracks[i]->m_armors.back();
                    cv::line(imageTemp, armor.m_topLeft, armor.m_topRight,
                             colorTemp, 1, 8, 0);
                    cv::line(imageTemp, armor.m_bottomLeft, armor.m_bottomRight,
                             colorTemp, 1, 8, 0);
                    cv::line(imageTemp, armor.m_topLeft, armor.m_bottomLeft,
                             colorTemp, 1, 8, 0);
                    cv::line(imageTemp, armor.m_topRight, armor.m_bottomRight,
                             colorTemp, 1, 8, 0);
                    cv::putText(imageTemp, std::to_string(armor.m_pattern),
                                armor.m_centerUV, cv::FONT_HERSHEY_SIMPLEX, 1,
                                colorTemp, 1);
                }
            }
        }
        // 三个线程1用来抓图有个问题要把实例化和重新启动相机的部分都放在函数里面吗
        //          1>如果实例化放在线程里面就要  图片是共享量
        //          在处理的时候要对其上锁
        //          处理的程序就是用算法部分初始化类输入图片   同时计算帧率
        //          将帧率打在屏幕上然后解锁
        //       2用来显示图片 控制频率就好 不需要每张图片都显示
        //       3用来交互控制另外两个线程 一部分是和用户交互
        //       另一部分是通过交互的结果来控制线程 保证实时接受键盘输入信号
        //       同时判断信号来作停止开始和结束选择
        //       可以通过睡眠来控制显示图片的时间 同时可以通过上锁解锁获数据
